#include "../include/speed_planner.hpp"

SpeedPlanner::SpeedPlanner(/* args */)
{
}

SpeedPlanner::~SpeedPlanner()
{
}

SpeedPlanner::SpeedPlanner(const Location &plan_start, const SLPoint &plan_start_sl, 
                 const vector<Location> &dp_path, const vector<SLPoint> &dp_path_sl) {
    this->plan_start_ = plan_start;
    this->plan_start_sl_ = plan_start_sl;
    this->dp_path_.assign(dp_path.begin(), dp_path.end());
    this->dp_path_sl_.assign(dp_path_sl.begin(), dp_path_sl.end());

    // 计算速度规划起点的 s_dot s_ddot
    Eigen::Vector2d tor(sin(plan_start.heading), cos(plan_start.heading));
    Eigen::Vector2d speed(plan_start.v_x, plan_start.v_y);
    Eigen::Vector2d accel(plan_start.a_x, plan_start.a_y);
    this->start_.t = 0;
    this->start_.s = 0;
    this->start_.s_dot = plan_start.v_x / sin(plan_start.heading);
    // this->start_.s_dot = tor.dot(speed);
    // this->start_.s_ddot = tor.dot(accel);

    // 计算每个点对应的 s
    double dis = 0.5;
    for (int i = 0; i < this->dp_path_sl_.size(); i++) {
        this->dp_path_sl_[i].s = dis * i;
    }

    // 对ST图进行非均匀采样， s方向采样 20 个点，t方向采样 16个点
    // 一共四种采样间隔，0.5 1.5 2.5 3.5
    double delta = 1.0;
    double tatol_s = 0;
    s_list.push_back(0);
    for (int i = 1; i < 20; i++) {
        if (i % 10 == 0 &&i >= 10) {
            delta += 1;
        }
        tatol_s += delta;
        s_list.push_back(tatol_s);
    }
    // 对于时间 t 均匀采样每隔 0.5 秒采样一个点
    for (int i = 1; i <= 16; i++) {
        this->t_list.push_back(i * 0.5);
    }
}


bool SpeedPlanner::genSTMap(const vector<Location> &obs_vec, 
                            const vector<SLPoint> &obs_vec_sl, vector<STMap> &st_vec) {
    double rLength = 1.5;       // 道路中心到左右边界的长度
    for (int i = 0; i < obs_vec.size(); i++) {
        if (obs_vec[i].speed < 0.2)
            // 对于速度太慢的障碍物，这里忽略不考虑
            continue;
        double to_zero = - obs_vec_sl[i].l / obs_vec[i].speed;
        STMap st;
        double t_min, t_max;
        if (to_zero < 0) {
            // 从左向右运动
            if (abs(obs_vec_sl[i].l) < rLength) {
                // 当前障碍物在道路上
                t_min = 0;
                t_max = abs(to_zero) + rLength / obs_vec[i].speed;
            } else {
                // 当前障碍物在道路外面
                t_min = abs(to_zero) - rLength / obs_vec[i].speed;
                t_max = abs(to_zero) + rLength / obs_vec[i].speed;
            }
        } else {
            // 障碍物从右向左运动
            if (abs(obs_vec_sl[i].l) < rLength) {
                t_min = 0;
                t_max = abs(to_zero) + rLength / obs_vec[i].speed;
            } else {
                t_min = abs(to_zero) - rLength / obs_vec[i].speed;
                t_max = abs(to_zero) + rLength / obs_vec[i].speed;
            }
        }
        st.t_in = t_min;
        st.t_out = t_max;
        st.s_in = obs_vec_sl[i].s;
        st.s_out = obs_vec_sl[i].s;
        st_vec.push_back(st);
    }
    return true;
}


/**
 * dp[i][j] 表示从起点到i行j列的代价
 * s_dot[i][j] 表示从起点到i行j列的代价
*/
bool SpeedPlanner::dp_decision(const vector<STMap> &st_vec, 
                               vector<double> &s_vec, 
                               vector<double> &v_vec,
                               vector<double> &t_vec) {
    int row_num = s_list.size();
    int col_num = t_list.size();

    vector<vector<double>> dp(col_num, vector<double>(row_num));
    vector<vector<double>> s_dot(col_num, vector<double>(row_num));

    // 对于第 1 列 col = 0
    for (int i = 0; i < row_num; i++) {
        STInfo begin = this->start_;
        STInfo end = this->calcSTCoordinate(0, i);
        this->calcuSDotAndDDot(begin, end);
        s_dot[0][i] = end.s_dot;
        dp[0][i] =  this->calcuDpCost(begin, end, st_vec);
    }
    // 首先遍历每一列
    for (int col = 1; col < col_num; col++) {
        auto min_ptr = min_element(dp[col-1].begin(), dp[col-1].end());
        int pre_row = min_ptr - dp[col-1].begin();
        // 获取上一列cost最小的ST点
        STInfo begin = this->calcSTCoordinate(col-1, pre_row);
        begin.s_dot = s_dot[col-1][pre_row];
        for (int row = 0; row < row_num; row++) {
            STInfo end = this->calcSTCoordinate(col, row);
            this->calcuSDotAndDDot(begin, end);
            s_dot[col][row] = end.s_dot;
            dp[col][row] = *min_ptr + this->calcuDpCost(begin, end, st_vec);
        }
    }

    vector<double> s_vec_tmp, t_vec_tmp;
    // 提取规划结果，找到每一列cost最小的点
    s_vec_tmp.push_back(this->start_.s);
    t_vec_tmp.push_back(this->start_.t);
    // v_vec.push_back(this->start_.s_dot);
    int col_end = col_num;      // 默认规划结束为最后一列

    // 查找最后一列的最小 cost
    auto right_min_ptr = min_element(dp[col_num-1].begin(), dp[col_num-1].end());

    // 循环最上层，找到最小cost最小值
    double min_value = 2 *1e6;
    int min_col_index = col_num;
    for (int col = 0; col < col_num; col++) {
        if (dp[col][0] < min_value) {
            min_value = dp[col][0];
            min_col_index = col;
        }
    }

    if (min_value > *right_min_ptr) {
        // 最小值在最右列，完整规划了 8s
        std::cout << " Right " << std::endl;
    } else {
        // 最小值在最上端，没有规划 8s
        col_end = min_col_index + 1;
    }

    for (int col = 0; col < col_end; col++) {
        auto min_ptr = min_element(dp[col].begin(), dp[col].end());
        int min_row = min_ptr - dp[col].begin();
        STInfo point = this->calcSTCoordinate(col, min_row);
        s_vec_tmp.push_back(point.s);
        t_vec_tmp.push_back(point.t);
        // v_vec.push_back(s_dot[col][min_row]);
    }
    // std::cout << "t_vec_tmp: " << " ";
    // for (int i = 0; i < t_vec_tmp.size(); i++) {
    //     std::cout << t_vec_tmp[i] << " ";
    // }
    // std::cout << std::endl << "s_vec_tmp: ";
    // for (int i = 0; i < s_vec_tmp.size(); i++) {
    //     std::cout << s_vec_tmp[i] << " ";
    // }
    // std::cout << std::endl;

    /* 对s t 信息进行增密 */
    for (int i = 0; i < s_vec_tmp.size() - 1; i++) {
        double cur_s = s_vec_tmp[i];
        double cur_t = t_vec_tmp[i];
        double nxt_s = s_vec_tmp[i+1];
        double nxt_t = t_vec_tmp[i+1];
        double k = (nxt_s - cur_s) / (nxt_t - cur_t);
        // 这里 0.5 表示规划路径的点的间隔
        int point_num = int((nxt_s - cur_s) / 0.5);
        // 这里的 0.5 表示t轴采样间隔
        double dt = 0.5 / point_num;
        for (int j = 0; j < point_num; j++) {
            s_vec.push_back(cur_s + j * dt * k);
            t_vec.push_back(cur_t + j * dt);
        }
    }
    // 计算速度
    for (int i = 0; i < s_vec.size() - 1; i++) {
        double v_tmp = (s_vec[i+1] - s_vec[i]) / (t_vec[i+1] - t_vec[i]);
        v_vec.push_back(v_tmp);
    }

    return true;
}

void SpeedPlanner::calcuSDotAndDDot(const STInfo &begin, STInfo &end) {
    double delta_t = end.t - begin.t;
    end.s_dot = (end.s - begin.s) / delta_t;
    end.s_ddot = (end.s_dot - begin.s_dot) / delta_t;
}

/**
 * 计算点begin到end的边的代价
*/
double SpeedPlanner::calcuDpCost(const STInfo &begin, 
                                 const STInfo &end, 
                                 const vector<STMap> &st_vec) {
    // 代价参数
    double ref_speed = 3;
    double w_cost_ref_speed = 100;
    double w_cost_accel = 2;
    double w_cost_obs = 1e6;

    double s_dot = (end.s - begin.s) / (end.t - begin.t);
    double s_ddot = (end.s_dot - begin.s_dot) / (end.t - begin.t);
    // 推荐速度代价
    double cost_ref_speed = w_cost_ref_speed * pow(s_dot - ref_speed, 2);
    // 加速度代价
    double cost_accel = 0;
    if (s_ddot < 5 && s_ddot > -5) {
        cost_accel = w_cost_accel * pow(s_ddot - 5, 2);
    } else {
        cost_accel = 2 * w_cost_accel * pow(s_ddot - 5, 2);
    }
    double cost_obs = this->calcuObsCost(begin, end, st_vec, w_cost_obs);
    return cost_ref_speed + cost_accel + cost_obs;
}


double SpeedPlanner::calcuObsCost(const STInfo &begin, const STInfo &end, 
                                  const vector<STMap> &st_vec, double w_cost_obs)
{   
    double cost_obs = 0;
    // 每条边采样5个点
    int n = 5;
    // 采样间隔
    double dt = (end.t - begin.t) / (n - 1);
    // 边的斜率
    double k = (end.s - begin.s) / (end.t - begin.t);
    for (int i = 0; i < n; i++) {
        // 计算采样点的坐标
        double t = begin.t + i * dt;
        double s = begin.s + k * i * dt;
        for (int j = 0; j < st_vec.size(); j++) {
            // 计算点到st线段的最短距离
            Eigen::Vector2d vec1(st_vec[j].s_in - s, st_vec[j].t_in -t);
            Eigen::Vector2d vec3(st_vec[j].s_out - s, st_vec[j].t_out - t);
            Eigen::Vector2d vec2 = vec3 - vec1;
            double min_dis = 0;
            double dis1 = vec1.norm();
            double dis2 = vec3.norm();
            double dis3 = sqrt(pow(vec1.norm(), 2) - pow(vec1.dot(vec2), 2));
            if ((vec1.dot(vec2) > 0 && vec3.dot(vec2) > 0) || 
                (vec1.dot(vec2) < 0 && vec3.dot(vec2) < 0)) {
                min_dis = min(dis1, dis2);
            } else 
                min_dis = dis3;
            cost_obs += this->calcuCollisionCost(w_cost_obs, min_dis);
        }
    }
    return 0;
}


double SpeedPlanner::calcuCollisionCost(double w_cost_obs, double min_dis) {
    double cost_collision = 0;
    if (abs(min_dis) < 5) {
        cost_collision = w_cost_obs;
    } else if (abs(min_dis) >= 5 && abs(min_dis) < 10) {
        cost_collision = 50000 / min_dis;
    } else {
        cost_collision = 0;
    }
    return cost_collision;
}


/**
 * 计算矩阵节点的 s t 坐标
 * 输入 row col 节点在矩阵的行号和列号
*/
STInfo SpeedPlanner::calcSTCoordinate(const int col, const int row)
{
    STInfo st_point;
    int m = s_list.size();
    // 左上角 (0, 0) 表示 s 的最大值 t 的最小值
    st_point.s = s_list[m - 1 - row];
    st_point.t = t_list[col];
    return st_point;
}

